package com.yuneec.droneservice.command;

import com.MAVLink.yuneec.msg_mav_rc_cmd_ack;
import com.yuneec.droneservice.model.ControllerLocation;
import com.yuneec.droneservice.request.IResponse;

/**
 * Created by gjw on 2017/3/24.
 */

public class GetControllerGPS extends BaseCommand {

    public GetControllerGPS() {
        super(MAV_RC_COMMAND.MAV_RC_CMD_GET_GPS);
    }

    @Override
    protected short[] body() {
        return null;
    }

    @Override
    public IResponse<ControllerLocation> toResponse(msg_mav_rc_cmd_ack ack) {
        int code = ack.code;
        IResponse<ControllerLocation> response = new IResponse(ids(), code);
        if (code == MAV_RC_ERROR_CODE.MAV_RC_ERR_SUCCESS) {
            try {
                if (ack.params == null || ack.params.length < 17) {
                    response.setCode(MAV_RC_ERROR_CODE.MAV_RC_ERR_UNKNOW);
                    return response;
                }
                int index = 0;
                ControllerLocation controllerLocation = new ControllerLocation();
                controllerLocation.longitude = uIntArrayToUInt(ack.params, index) / 1e7f;
                index += 4;
                controllerLocation.latitude = uIntArrayToUInt(ack.params, index) / 1e7f;
                index += 4;
                controllerLocation.altitude = uIntArrayToUShort(ack.params, index) / 100f;
                index += 2;
                controllerLocation.satelliteCount = ack.params[index++];
                controllerLocation.accuracy = uIntArrayToUShort(ack.params, index);
                index += 2;
                controllerLocation.speed = uIntArrayToUShort(ack.params, index);
                index += 2;
                controllerLocation.angle = uIntArrayToUShort(ack.params, index);
                response.setParam(controllerLocation);
            } catch (Exception e) {
                response.setCode(MAV_RC_ERROR_CODE.MAV_RC_ERR_UNKNOW);
            }
        }
        return response;
    }

}
